﻿#include "dmc_b140_m.h"

#include <QtNetwork>
#include <QtSerialPort/QSerialPort>
#include <QtSerialPort/QSerialPortInfo>
#include <QDebug>
#include <QString>
#include <QMessageBox>

//const int BUF_SIZE = 40;44
static const int AXIS_STATUS_STILLNESS = 0;
static const int AXIS_STATUS_PMOVE = 1;
static const int AXIS_STATUS_GOHOME = 2;
const int AXIS_MAX = 4;

DMC_B140_M::DMC_B140_M(QObject *parent)
    : MotionController(parent),
      m_io(nullptr),
      m_socket(nullptr),
      m_serial(nullptr)
{
    m_serial = new QSerialPort(this);
    m_socket = new QTcpSocket(this);

    m_isTCPPortOK = false;
    m_isSerialPortOK = false;

    for(int i = 0; i < 4; i++)
    {
        m_speed[i] = 0;
        m_encoderPos[i] = 0;
        m_theoryPos[i] = 0;

        m_axisStatus[i] = AXIS_STATUS_STILLNESS;
        m_llimit[i] = false;
        m_rlimit[i] = false;
        m_home[i] = false;
    }

    m_pTimer = new QTimer(this);
#ifdef NDEBUG
    m_pTimer->setInterval(100);
#else
    m_pTimer->setInterval(1000);
#endif


    connect(m_pTimer, &QTimer::timeout, this, &DMC_B140_M::on_timeout);

    connect(m_socket, SIGNAL(connected()), this, SLOT(on_TCPConnected()));
    connect(m_socket, SIGNAL(disconnected()), this, SLOT(on_disconnected()));
}

bool DMC_B140_M::isValid()
{
    if(m_io == m_socket)
        return m_isTCPPortOK;
    else if(m_io == m_serial)
        return m_isSerialPortOK;
    return false;
}


bool DMC_B140_M::readIOBit(int bitNo, bool &value)
{
    int state;
    QString cmd = QString("TI\r");
    if( !cmdI(cmd, state) )
    {
        return false;
    }
    if(bitNo < 0 || bitNo > 7)
    {
        m_errno = MTCTR_BITNO;
        return false;
    }
    if( (1 << bitNo) & state )
    {
        value = 0xff;
    }
    else
    {
        value = 0x00;
    }
    return true;
}

bool DMC_B140_M::readIOPort(int port, uint32_t &value)
{
    Q_UNUSED(port); // B140 只有一组IO，8 个 IN， 4 个 OUT
    QString cmd = QString("TI\r");
    return cmdI(cmd, reinterpret_cast<int &>(value));
}

bool DMC_B140_M::writeIOBit(int bitNo, bool value)
{
//    qDebug() << "writeIOBit bitNo = " << bitNo <<", value =" << value;
    if(bitNo < 0 || bitNo > 3)
    {
        m_errno = MTCTR_BITNO;
        return false;
    }
    if(value)
    {
        QString cmd = QString("SB%1\r").arg((bitNo + 1));
//        qDebug() << cmd;
        return cmdV(cmd);
    }
    else
    {
        QString cmd = QString("CB%1\r").arg((bitNo + 1));
//        qDebug() << cmd;
        return cmdV(cmd);
    }
}

bool DMC_B140_M::writeIOPort(int port, uint32_t value)
{
    Q_UNUSED(port);
    QString cmd = QString("OP %1\r").arg(value);
    return cmdV(cmd);
}

bool DMC_B140_M::setPWMOut(int channel, double freq, double duty)
{
    Q_UNUSED(channel);
    Q_UNUSED(freq);
    Q_UNUSED(duty);
    return false;
}

bool DMC_B140_M::PWMValue(int channel, double &freq, double &duty)
{
    Q_UNUSED(channel);
    Q_UNUSED(freq);
    Q_UNUSED(duty);
    return false;
}

bool DMC_B140_M::setSoftLlimit(unsigned short axis, int32_t llimit)
{
    if(axis > 3)
    {
        m_errno = MTCTR_AXIS_NO;
        return false;
    }
    QString cmd = QString("BL%1=%2\r").arg(static_cast<char>(axis + 'A')).arg(llimit);

    return cmdV(cmd);
}

bool DMC_B140_M::setSoftRlimit(unsigned short axis, int32_t rlimit)
{
    if(axis > 3)
    {
        m_errno = MTCTR_AXIS_NO;
        return false;
    }
    QString cmd = QString("FL%1=%2\r").arg(static_cast<char>(axis + 'A')).arg(rlimit);

    return cmdV(cmd);
}

bool DMC_B140_M::cmdV(QString cmd, char desiredReturn)
{
//    qDebug() << "DMC_B140_M:" << cmd;
    if(!m_io)
    {
        m_errno = MTCTR_UNREACHABLE;
        return false;
    }
    if(m_io->isWritable())
    {
        m_io->write(cmd.toLocal8Bit());
        if(!m_io->waitForReadyRead(50))
        {
            m_errno = MTCTR_TIMEOUT;
            return false;
        }
        m_received = m_io->readAll();
        if(m_received.size() != 1)
        {
            m_errno = MTCTR_UNKNOWN;
            return false;
        }
        else if(m_received.at(0) == '?')
        {
            m_errno = MTCTR_BAD_CMD;
            return false;
        }
        else if(m_received.at(0) != desiredReturn)
        {
            m_errno = MTCTR_UNKNOWN;
            return false;
        }
        else
        {
            m_errno = MTCTR_NOERR;
            return true;
        }
    }
    m_errno = MTCTR_UNREACHABLE;
    return false;
}

bool DMC_B140_M::cmdD(QString cmd, double &value)
{
    if(!m_io)
    {
        m_errno = MTCTR_UNREACHABLE;
        return false;
    }
    if(m_io->isWritable())
    {
        m_io->write(cmd.toLocal8Bit());
        if(!m_io->waitForReadyRead(50))
        {
            m_errno = MTCTR_TIMEOUT;
            return false;
        }
        m_received = m_io->readAll();
        if(m_received.at(0) == '?')
        {
            m_errno = MTCTR_BAD_CMD;
            return false;
        }
        QString data = m_received;
        data.remove(QChar(':'));
        data = data.trimmed();
        bool ok;
        value = data.toDouble(&ok);
        if(!ok) m_errno = MTCTR_UNKNOWN;
        return ok;
    }
    else
    {
        m_errno = MTCTR_UNREACHABLE;
        return false;
    }
}

bool DMC_B140_M::cmdI(QString cmd, int &value)
{
//    qDebug() << "DMC_B140_M:" << cmd;
    if(!m_io)
    {
        m_errno = MTCTR_UNREACHABLE;
        return false;
    }
    value = 0;
    if(m_io->isWritable())
    {
        m_io->write(cmd.toLocal8Bit());
        if(!m_io->waitForReadyRead(50))
        {
            m_errno = MTCTR_TIMEOUT;
            return false;
        }
        m_received = m_io->readAll();
        if(m_received.at(0) == '?')
        {
            m_errno = MTCTR_BAD_CMD;
            return false;
        }
        QString data = m_received;
        data.remove(QChar(':'));
        data = data.trimmed();
        bool ok;
        value = data.toInt(&ok);
        if(!ok) m_errno = MTCTR_UNKNOWN;
        return ok;
    }
    else
    {
        m_errno = MTCTR_UNREACHABLE;
        return false;
    }
}

bool DMC_B140_M::isMoving(unsigned short axis, bool &state)
{
    if(axis > 3)
    {
        m_errno = MTCTR_AXIS_NO;
        return false;
    }
    double value = 0;
    QString cmd = QString("MG _BG%1\r").arg(static_cast<char>(axis + 'A'));
    bool ret = cmdD(cmd, value);
    state = (value == 1.0);
    return ret;
}

bool DMC_B140_M::isSMoving(bool &state)
{
    double value = 0;
    QString cmd = QString("MG _BGS\r");
    bool ret = cmdD(cmd, value);
    state = (value == 1.0);
    return ret;
}

bool DMC_B140_M::isReverseLimit(unsigned short axis, bool &state)
{
    double value;
    QString cmd = QString("MG _LR%1\r").arg(static_cast<char>(axis + 'A'));
    bool ret = cmdD(cmd, value);
    state = (value == 1.0);
    return ret;
}

bool DMC_B140_M::isForwardLimit(unsigned short axis, bool &state)
{
    double value;
    QString cmd = QString("MG _LF%1\r").arg(static_cast<char>(axis + 'A'));
    bool ret = cmdD(cmd, value);
    state = !(value == 1.0);
    return ret;
}

bool DMC_B140_M::isHome(unsigned short axis, bool &state)
{
    QString cmd = QString("SC%1\r").arg(static_cast<char>(axis + 'A'));
    int32_t value;
    bool ret = cmdI(cmd, value);
    state = (value == 10);
    return ret;
}

bool DMC_B140_M::readLimitStatus(unsigned short axis, bool &llimit, bool &rlimit, bool &home)
{
    double value[3];
    QString cmd = QString("MG _LR%1,_LF%1,_HM%1\r").arg(static_cast<char>(axis + 'A'));
    bool ret = cmdD3(cmd, value, ' ');

    llimit = !(value[0] == 1);
    rlimit = !(value[1] == 1);
    home = !(value[2] == 1);

    return ret;
}

bool DMC_B140_M::readAxisEMStop(bool emstop[])
{
    int value;
    QString cmd = QString("TI\r");
    if(! cmdI(cmd, value)) return false;

    emstop[0] = !(value & 0b00010000);
    emstop[1] = !(value & 0b00100000);
    emstop[2] = !(value & 0b01000000);
    emstop[3] = !(value & 0b10000000);
    return true;
}

bool DMC_B140_M::isEMStop(bool &state)
{
    double value;
    QString cmd = QString("MG _AB\r");
    if(! cmdD(cmd, value)) return false;

    state = !(value == 1.0);
    return true;
}

bool DMC_B140_M::cmdI4(QString cmd, int value[4], char sep)
{
//    qDebug() << "DMC_B140_M:" << cmd;
    if(!m_io)
    {
        m_errno = MTCTR_UNREACHABLE;
        return false;
    }
    if(m_io->isWritable())
    {
        m_io->write(cmd.toLocal8Bit());
        if(!m_io->waitForReadyRead(50))
        {
            m_errno = MTCTR_TIMEOUT;
            return false;
        }
        else
        {
            m_received = m_io->readAll();
            if(m_received.at(0) == '?')
            {
                m_errno = MTCTR_BAD_CMD;
                return false;
            }
            QString data = m_received;
            data.remove(QChar(':'));
            data = data.trimmed();
            QStringList pos = data.split(sep);
            if(pos.size() == 4)
            {
                for(int i = 0; i < 4; i++)
                {
                    value[i] = pos[i].toInt();
                }
                return true;
            }
        }
    }
    m_errno = MTCTR_UNREACHABLE;
    return false;
}

bool DMC_B140_M::stopCode(unsigned short axis,  int32_t &value)
{
    QString cmd = QString("SC%1\r").arg(static_cast<char>(axis + 'A'));
    return cmdI(cmd, value);
}

bool DMC_B140_M::stopCode(int32_t value[])
{
    QString cmd = QString("SCA,B,C,D\r");
    return cmdI4(cmd, value);
}

void DMC_B140_M::updatePosition()
{
    int32_t theoryPos[AXIS_MAX], encodePos[AXIS_MAX];
    readTheoryPosAll(theoryPos);
    readEncoderPosAll(encodePos);
    bool isUpdate = false;
    for(int i = 0; i < m_axis; i++)
    {
        if(theoryPos[i] != m_theoryPos[i])
        {
            qDebug() <<"theoryPos[i] = " << theoryPos[i] << ", m_theoryPos[i] = " << m_theoryPos[i] << ", axis = " << i;
            m_theoryPos[i] = theoryPos[i];

            isUpdate = true;
        }
        if(encodePos[i] != m_encoderPos[i])
        {
            qDebug() <<"encodePos[i] = " << encodePos[i] << ", m_encoderPos[i] = " << m_encoderPos[i] << ", axis = " << i;
            m_encoderPos[i] = encodePos[i];
            isUpdate = true;
        }
    }
    //qDebug() << "isUpdate = " << isUpdate;
    if(isUpdate)
    {
        qDebug() << "emit SMC106A::positionChanged";
        emit positionChanged(theoryPos, encodePos);
    }
}

void DMC_B140_M::updateAxisStatus()
{
    int32_t value;
    for(int axis = 0; axis < m_axis; axis++)
    {

        if( m_axisStatus[axis] == AXIS_STATUS_PMOVE) //pmove 状态
        {
            bool state;
            isMoving(axis, state);
            if(!state)
            {
                stopCode(axis, value);
                m_axisStatus[axis] = 0;// 已停止。
                emit axisStoped(axis, value);
            }
        }
        else if(m_axisStatus[axis] == AXIS_STATUS_GOHOME) //GoHome 状态
        {
            bool state;
            isMoving(axis, state);
            if(!state)
            {
                stopCode(axis, value);
                m_axisStatus[axis] = 0;// 已停止。
                if(value == 10)
                {
                    emit goHomeFinished(axis);
                }
                else
                {
                    emit axisStoped(axis, value);
                }
            }
        }
    }
    if(m_multicoorStatus[0] == 1)
    {
        bool state;
        isSMoving(state);
        m_multicoorStatus[0] = 0;
        emit lineMoveStoped(0);
    }
}

void DMC_B140_M::updateLimitStatus()
{
    for(unsigned short i = 0; i < m_axis; i++)
    {
        bool llimit, rlimit, home;
        readLimitStatus(i, llimit, rlimit, home);
        if(llimit != m_llimit[i] || rlimit != m_rlimit[i] || home != m_home[i] )
        {
            m_llimit[i] = llimit;
            m_rlimit[i] = rlimit;
            m_home[i] = home;
            qDebug() << "limitStatusChanged , axis = " << i << "llimit = " << llimit << ", rlimit = " << rlimit;
            emit limitStatusChanged(i, llimit, rlimit, home);
        }
    }
}

void DMC_B140_M::updateINPort()
{
    uint32_t value;
    readIOPort(0, value);
    if(value != m_io_in)
    {
        m_io_in = value;
        emit INPortChanged(0, value);
    }
}

bool DMC_B140_M::goHome(unsigned short axis, int speed)
{
    QString cmd = QString("SP%1=%2\r").arg(static_cast<char>(axis + 'A')).arg(speed);
    cmdV(cmd);
    cmd = QString("HM%1\r").arg(static_cast<char>(axis + 'A'));
    cmdV(cmd);
    cmd = QString("BG%1\r").arg(static_cast<char>(axis + 'A'));
    m_homeSpeed[axis] = speed;
    return cmdV(cmd);
}

bool DMC_B140_M::setHomeSpeed(unsigned short axis, int speed)
{
    m_homeSpeed[axis] = speed;
    return true;
}


bool DMC_B140_M::readTheoryPosAll(int value[4])
{
    QString cmd = QString("TD\r");
    if( cmdI4(cmd, value) )
    {
//        for(int i = 0; i < 4; i++)
//        {
//            m_theoryPos[i] = value[i];
//        }
        return true;
    }
    return false;
}

bool DMC_B140_M::setStartSpeed(unsigned short axis, int startSpeed)
{
    //TODO: 暂时没有实现这个功能
    Q_UNUSED(axis);
    Q_UNUSED(startSpeed);
    return true;
}

bool DMC_B140_M::setStopSpeed(unsigned short axis, int stopSpeed)
{
    //TODO: 暂时没有实现这个功能
    Q_UNUSED(axis);
    Q_UNUSED(stopSpeed);
    return true;
}

bool DMC_B140_M::readEncoderPos(unsigned short axis, int32_t &value)
{
    QString cmd = QString("TP%1\r").arg(static_cast<char>(axis + 'A'));
    return cmdI(cmd, value);
}

bool DMC_B140_M::readEncoderPosAll(int value[])
{
    QString cmd = QString("TP\r");
    if( cmdI4(cmd, value) )
    {
//        for(int i = 0; i < 4; i++)
//        {
//            m_encoderPos[i] = value[i];
//        }
        return true;
    }
    return false;
}


DMC_B140_M::~DMC_B140_M()
{
    if(!m_io) return;
    abort();
    cmdV("CB 1\r");

    if(m_io == m_socket)
    {
        disconnect(m_socket, SIGNAL(disconnected()), this, SLOT(on_disconnected()));
        m_socket->disconnectFromHost();
    }
    else
    {
        m_serial->close();
    }
}

bool DMC_B140_M::setTheoryPos(unsigned short axis, int32_t offset)
{

    QString cmd = QString("DP%1=%2\r").arg(static_cast<char>(axis + 'A')).arg(offset);
    qDebug() << "setTheoryPos: " << cmd;
    return cmdV(cmd);
}

bool DMC_B140_M::setEncoderPos(unsigned short axis, int32_t offset)
{
    QString cmd = QString("DE%1=%2\r").arg(static_cast<char>(axis + 'A')).arg(offset);
    return cmdV(cmd);
}

bool DMC_B140_M::connectViaSerialPort(QString commName,
                                      int baudrate,
                                      char parity,
                                      int dataBits,
                                      int stopBits)
{
    m_serial->setPortName(commName);
    m_serial->setBaudRate(baudrate);

    switch (parity)
    {
    case 'E':
        m_serial->setParity(QSerialPort::EvenParity);
        break;
    case 'O':
        m_serial->setParity(QSerialPort::OddParity);
        break;
    default:
        m_serial->setParity(QSerialPort::NoParity);
        break;
    }

    switch (dataBits)
    {
    case 7:
        m_serial->setDataBits(QSerialPort::Data7);
        break;
    default:
        m_serial->setDataBits(QSerialPort::Data8);
        break;
    }

    switch (stopBits)
    {
    case 2:
        m_serial->setStopBits(QSerialPort::TwoStop);
        break;
    case 3:
        m_serial->setStopBits(QSerialPort::OneAndHalfStop);
        break;
    default:
        m_serial->setStopBits(QSerialPort::OneStop);
        break;
    }

    m_serial->setFlowControl(QSerialPort::NoFlowControl);
    if( m_serial->open(QIODevice::ReadWrite) )
    {
        if(!m_serial->waitForReadyRead(50))
        {
            m_errno = MTCTR_UNREACHABLE;
            m_isSerialPortOK = false;
            qWarning() << "TIMEOUT, communication with B140 is failed!";
            return false;
        }
        m_received = m_serial->readAll();
        if(m_received.at(0) != ':')
        {
            m_isSerialPortOK = false;
            m_errno = MTCTR_UNREACHABLE;
            qWarning() << "no colon(:) received, communication with B140 is failed!";
            return false;
        }
        m_io = m_serial;
        m_isSerialPortOK = true;
        m_errno = MTCTR_NOERR;
        qInfo() << "tcp comunication with B140 is successful!";
        emit connectionSetup();
        init();
        return true;
    }
    return false;
}

bool DMC_B140_M::connectViaEthernet(QString ip, uint16_t port)
{
    m_isTCPPortOK = false;
    if(m_socket && !ip.isEmpty())
    {
        m_socket->connectToHost(ip, port, QTcpSocket::ReadWrite);
        m_io = m_socket;
        qDebug() << "m_socket->connectToHost(ip, port, QTcpSocket::ReadWrite)";
    }
    else
    {
        m_errno = MTCTR_UNKNOWN;
        m_isTCPPortOK = false;
        return false;
    }
    QTimer::singleShot(1000, this, &DMC_B140_M::on_TCPConnectTimeout);

//    if(!m_socket->waitForConnected(100) )
//    {
//        qDebug() << m_socket->errorString();
//        m_errno = MTCTR_TIMEOUT;
//        m_isTCPPortOK = false;
//        return false;
//    }
//    else
//    {
//        m_errno = MTCTR_NOERR;
//        m_isTCPPortOK = true;
//        return true;
//    }
    return true;
}

void DMC_B140_M::closeEthernet()
{
    if(m_socket)
    {
        m_socket->close();
    }
    m_isTCPPortOK = false;
}

void DMC_B140_M::closeSerialPort()
{
    if(m_serial)
    {
        m_serial->close();
    }
    m_isSerialPortOK = false;
}

void DMC_B140_M::init()
{
    m_pTimer->start();
    /// TODO: 这里应该填加些操作。
}

bool DMC_B140_M::action1()
{
    cmdV("LD2,2,0,0\r");
    return true;
}

bool DMC_B140_M::action2()
{
    cmdV("LD0,0,0,0\r");
    return true;
}


void DMC_B140_M::on_TCPConnected()
{
    qDebug() << "B140 TCP connect ok!";
    m_socket->write(QString("\r").toLocal8Bit());
    if(!m_socket->waitForReadyRead(1000))
    {
        m_errno = MTCTR_UNREACHABLE;
        m_isTCPPortOK = false;
        qWarning() << "TIMEOUT, communication with B140 is failed!";
        return;
    }
    m_received = m_socket->readAll();
    if(m_received.at(0) != ':')
    {
        qWarning() << "no colon(:) received, communication with B140 is failed!";
        m_isTCPPortOK = false;
        m_errno = MTCTR_UNREACHABLE;
        return;
    }
    m_isTCPPortOK = true;
    qInfo() << "tcp comunication with B140 is successful!";
    emit connectionSetup();
    init();
}

void DMC_B140_M::on_TCPConnectTimeout()
{
    if(m_isTCPPortOK == false)
    {
        // 说明真的超时了, 这时关闭 socket
        closeEthernet();
        emit tcpConnectTimeout();
    }

}

bool DMC_B140_M::isTCPConnected()
{
    return m_isTCPPortOK;
}

void DMC_B140_M::on_disconnected()
{
    m_isTCPPortOK = false;
    m_pTimer->stop();
    emit connectionLost();
}

void DMC_B140_M::on_timeout()
{
    updateAxisStatus();
    updateINPort();
    updatePosition();
    updateLimitStatus();
}

bool DMC_B140_M::setSpeed(unsigned short axis, int speed)
{
    if(axis > 3)
    {
        m_errno = MTCTR_AXIS_NO;
        return false;
    }
    m_speed[axis] = speed;
    QString cmd = QString("SP%1=%2\r").arg(static_cast<char>(axis + 'A')).arg(speed);

    return cmdV(cmd);
}

/**
 * @brief DMC_B140_M::setAcceleration
 * @param axis
 * @param acceleration 为无符号整数，范围1024~1073740800，单位是脉冲每秒平方。
 *                     输入的数据如不是1024的倍数，回自动变换为不大于输入数值的1024倍数。
 *                     如输入的数据小于1024，则自动变换为1024。
 */
bool DMC_B140_M::setAcceleration(unsigned short axis, int acceleration)
{
    if(axis > 3)
    {
        m_errno = MTCTR_AXIS_NO;
        return false;
    }
    QString cmd = QString("AC%1=%2\r").arg(static_cast<char>(axis + 'A')).arg(acceleration);
    m_acceleration[axis] = acceleration;
    return cmdV(cmd);
}

/**
 * @brief DMC_B140_M::setDeceleration
 * @param axis
 * @param deceleration 为无符号整数，范围1024~1073740800，单位是脉冲每秒平方。
 *                     输入的数据如不是1024的倍数，回自动变换为不大于输入数值的1024倍数。
 *                     如输入的数据小于1024，则自动变换为1024。
 */
bool DMC_B140_M::setDeceleration(unsigned short axis, int deceleration)
{
    if(axis > 3)
    {
        m_errno = MTCTR_AXIS_NO;
        return false;
    }
    QString cmd = QString("DC%1=%2\r").arg(static_cast<char>(axis + 'A')).arg(deceleration);
    m_deceleration[axis] = deceleration;
    return cmdV(cmd);
}

bool DMC_B140_M::moveTo(unsigned short axis, int position)
{
    if(axis > 3)
    {
        m_errno = MTCTR_AXIS_NO;
        return false;
    }
    QString cmd = QString("PA%1=%2\r").arg(static_cast<char>(axis + 'A')).arg(position);
    cmdV(cmd);

    cmd = QString("BG%1\r").arg(static_cast<char>(axis + 'A')); 
    m_axisStatus[axis] = AXIS_STATUS_PMOVE;
    return cmdV(cmd);
}

bool DMC_B140_M::move(unsigned short axis, int dir)
{
    if(axis > 3)
    {
        m_errno = MTCTR_AXIS_NO;
        return false;
    }
    QString cmd;
    if(dir == 1)
    {
        cmd = QString("JG%1=%2\r").arg(static_cast<char>(axis + 'A')).arg(m_speed[axis]);
    }
    else
    {
        cmd = QString("JG%1=%2\r").arg(static_cast<char>(axis + 'A')).arg(-m_speed[axis]);
    }

    cmdV(cmd);

    cmd = QString("BG%1\r").arg(static_cast<char>(axis + 'A'));
    m_axisStatus[axis] = AXIS_STATUS_PMOVE;
    return cmdV(cmd);
}

bool DMC_B140_M::moveXYSteps(int stepx, int stepy, int spd)
{
    cmdV("LMAB\r");
    QString cmd = QString("VS %1\r").arg(spd);
    cmdV(cmd);
    cmd = QString("LI %1, %2\r").arg(stepx).arg(stepy);
    cmdV(cmd);
    m_multicoorStatus[0] = 1;
    return cmdV("BGS\r");
}

bool DMC_B140_M::moveSteps(unsigned short axis, int steps)
{
    if(axis > 3)
    {
        m_errno = MTCTR_AXIS_NO;
        return false;
    }

    QString cmd = QString("PR%1=%2\r").arg(static_cast<char>(axis + 'A')).arg(steps);
    cmdV(cmd);
    cmd = QString("BG%1\r").arg(static_cast<char>(axis + 'A'));
    m_axisStatus[axis] = AXIS_STATUS_PMOVE;
    return cmdV(cmd);
}

bool DMC_B140_M::home(unsigned short axis)
{
    return goHome(axis, m_homeSpeed[axis]);
}

bool DMC_B140_M::stopAxis(unsigned short axis)
{
    QString cmd;
    if(0<= axis && axis <= 3)
    {
        cmd = QString("ST%1\r").arg(static_cast<char>(axis + 'A'));
    }
    else
    {
        cmd = QString("ST\r");
    }

    return cmdV(cmd);
}

bool DMC_B140_M::stop()
{
    return cmdV("ST\r");
}

bool DMC_B140_M::abort()
{
    QString cmd = QString("AB\r");
    return cmdV(cmd);
}

bool DMC_B140_M::readTheoryPos(unsigned short axis, int32_t &value)
{
    QString cmd = QString("TD%1\r").arg(static_cast<char>(axis + 'A'));
    return cmdI(cmd, value);
}


bool DMC_B140_M::setEMStop(bool on)
{
    QString cmd;
    if(on)
    {
        cmd = QString("AX1\r");
        m_enableEMStop  = true;
    }
    else
    {
        cmd = QString("AX0\r");
        m_enableEMStop = false;
    }
    return cmdV(cmd);
}

bool DMC_B140_M::setAxisEMStop(bool on)
{
    QString cmd;
    if(on)
    {
        cmd = QString("CN,,,1\r");
        m_enableAxisEMStop  = true;
    }
    else
    {
        cmd = QString("CN,,,0\r");
        m_enableAxisEMStop = false;
    }
    return cmdV(cmd);
}

bool DMC_B140_M::cmdD2(QString cmd, double value[2], char sep)
{
    if(m_io && m_io->isWritable())
    {
        m_io->write(cmd.toLocal8Bit());
        if(!m_io->waitForReadyRead(50))
        {
            m_errno = MTCTR_TIMEOUT;
            return false;
        }
        else
        {
            m_received = m_io->readAll();
            if(m_received.at(0) == '?')
            {
                m_errno = MTCTR_BAD_CMD;
                return false;
            }
            QString data = m_received;
            data.remove(QChar(':'));
            data = data.trimmed();
            QStringList pos = data.split(sep);
            if(pos.size() == 2)
            {
                for(int i = 0; i < 2; i++)
                {
                    value[i] = pos[i].toDouble();
                }
                return true;
            }
        }
    }
    m_errno = MTCTR_UNREACHABLE;
    return false;
}

bool DMC_B140_M::cmdD3(QString cmd, double value[3], char sep)
{
    if(m_io && m_io->isWritable())
    {
        m_io->write(cmd.toLocal8Bit());
        if(!m_io->waitForReadyRead(50))
        {
            m_errno = MTCTR_TIMEOUT;
            return false;
        }
        else
        {
            m_received = m_io->readAll();
            //qDebug() << cmd << "," << m_received;
            if(m_received.at(0) == '?')
            {
                m_errno = MTCTR_BAD_CMD;
                return false;
            }
            QString data = m_received;
            data.remove(QChar(':'));
            data = data.trimmed();
            QStringList pos = data.split(sep);
            if(pos.size() == 3)
            {
                for(int i = 0; i < 3; i++)
                {
                    value[i] = pos[i].toDouble();
                }
                return true;
            }
        }
    }
    m_errno = MTCTR_UNREACHABLE;
    return false;
}

bool DMC_B140_M::cmdD4(QString cmd, double value[4], char sep)
{
    if(m_io && m_io->isWritable())
    {
        m_io->write(cmd.toLocal8Bit());
        if(!m_io->waitForReadyRead(50))
        {
            m_errno = MTCTR_TIMEOUT;
            return false;
        }
        else
        {
            m_received = m_io->readAll();
            if(m_received.at(0) == '?')
            {
                m_errno = MTCTR_BAD_CMD;
                return false;
            }
            QString data = m_received;
            data.remove(QChar(':'));
            data = data.trimmed();
            QStringList pos = data.split(sep);
            if(pos.size() == 4)
            {
                for(int i = 0; i < 4; i++)
                {
                    value[i] = pos[i].toDouble();
                }
                return true;
            }
        }
    }
    m_errno = MTCTR_UNREACHABLE;
    return false;
}

